package competition.cig.cs478.features;

import java.util.ArrayList;
import java.util.List;

import ch.idsia.mario.engine.sprites.Mario;
import ch.idsia.mario.environments.Environment;
import java.util.PriorityQueue;

public class FeatureParser {

	private static final byte ID_COIN = 34;
	private Environment prevObservation;
	private Environment curObservation;
	private long startTime;

	public FeatureParser() {
		startTime = System.currentTimeMillis();
		prevObservation = new NullEnvironment();
		curObservation = new NullEnvironment();
	}

	public double[] getMarioVelocity() {
		float[] lastPos = prevObservation.getMarioFloatPos();
		float[] curPos = curObservation.getMarioFloatPos();
		// 1.2 is marios max speed if running.
		double[] vel = { ((double) curPos[0] - (double) lastPos[0]) / 1.2,
				((double) curPos[1] - (double) lastPos[1]) / 1.2 };
		return vel;
	}

	public double getMarioState() {
		return curObservation.getMarioMode();
	}

	public double getClockTime() {
		return (System.currentTimeMillis() - startTime) / 200000.0;
	}

	public double getDistanceToFinish() {
		double levelLength = curObservation.getLevelLength();
		return (levelLength - (double) curObservation.getMarioFloatPos()[0])
				/ levelLength;
	}

	public double[] getHeights() {
		byte[][] levelInfo = curObservation.getLevelSceneObservationZ(1);
		double[] retVal = new double[10];
		int startX = Environment.HalfObsWidth;
		int startY = Environment.HalfObsHeight;

		// Get heights starting with first 7 ahead
		// first ground block block
		// printLevelInfo(levelInfo);

		for (int i = startX + 1; i <= startX + 7; i++) {
			int yPos = startY;
			if (i < 0 || yPos < 0)
				System.out.println("ERROR");
			// Search downwards first 21 means the bottom was reached
			while (yPos < Environment.HalfObsHeight * 2
					&& levelInfo[yPos][i] != -10 && levelInfo[yPos][i] != 20) {
				yPos++;
			}
			// Get back on
			yPos--;
			while (yPos > 0 && levelInfo[yPos][i] == -10 && levelInfo[yPos][i] == 20) {
				yPos--;
			}
			if (yPos == Environment.HalfObsHeight * 2
					&& levelInfo[yPos - 1][i] != -10 && levelInfo[yPos][i] != 20) {
				retVal[i - startX] = 0;
			} else {
				retVal[i - startX] = (double) yPos
						/ ((double) Environment.HalfObsHeight * 2.0);
			}
		}
		// Second we get what is behind him ground wise up to 3 blocks

		for (int i = startX - 1; i >= startX - 3; i--) {
			int yPos = startY;
			while (yPos < Environment.HalfObsHeight * 2
					&& levelInfo[yPos][i] != -10 && levelInfo[yPos][i] != 20) {
				yPos++;
			}
			yPos--;
			while (yPos >= 0 && levelInfo[yPos][i] == -10 && levelInfo[yPos][i] == 20) {
				yPos--;
			}
			if (yPos == Environment.HalfObsHeight * 2
					&& levelInfo[yPos - 1][i] != -10 && levelInfo[yPos][i] != 20) {
				retVal[(startX - i) + 7 - 1] = 0;
			} else {
				retVal[(startX - i) + 7 - 1] = (double) yPos
						/ ((double) Environment.HalfObsHeight * 2.0);

			}
		}
		return retVal;
	}

	private void printLevelInfo(byte[][] levelInfo) {

		System.out
				.println("*************************************************************************");
		for (int x = 0; x < levelInfo.length; x++) {
			for (int y = 0; y < levelInfo.length; y++) {
				System.out.printf("%3d ", levelInfo[x][y]);
			}
			System.out.println();
		}

	}

	/**
	 * Support function, Gets the three closest enemies
	 * 
	 * @return an array of the 3 closest enemies
	 */
	public Enemy[] getThreeClosestEnemies() {
		// Map (distance -> type), sorting by distance
		PriorityQueue<Enemy> pq = new PriorityQueue<Enemy>();
		byte[][] enemies = this.curObservation.getEnemiesObservation();
		for (int x = 0; x < 22; x++) {
			for (int y = 0; y < 22; y++) {
				pq.add(new Enemy(x, y, enemies[x][y]));
			}
		}

		Enemy[] ret = new Enemy[3];
		for (int i = 0; i < 3; i++) {
			if (pq.size() >= 0) {
				ret[i] = pq.remove();
			} else {
				ret[i] = null;
			}
		}
		return ret;
	}

	/**
	 * Gets the 3 closest enemy types
	 * 
	 * @return the type of the 3 closest enemies
	 */
	public double[] getEnemies() {
		Enemy[] enemies = this.getThreeClosestEnemies();
		double[] ret = new double[3];
		for (int i = 0; i < 3; i++) {
			if (enemies[i] != null) {
				ret[i] = enemies[i].getType();
			} else {
				ret[i] = 0;
			}
		}
		return ret;
	}

	/**
	 * Gets the x and y positions of the 3 closest enemies
	 * 
	 * @return double[] indicating the 3 x and y positions of the 3 closest
	 *         enemies normalized between 0 and 1. If there are not 3 enemies,
	 *         the distances of absent enemies is set to 1 for x and y
	 */
	public double[] getEnemyPositions() {
		Enemy[] enemies = this.getThreeClosestEnemies();
		double[] ret = new double[6];
		for (int i = 0; i < 3; i++) {
			if (enemies[i] != null) {
				ret[i * 2] = enemies[i].getxOffset();
				ret[i * 2 + 1] = enemies[i].getyOffset();
			} else {
				ret[i * 2] = 1.0;
				ret[i * 2 + 1] = 1.0;
			}
		}
		return ret;
	}

	/**
	 * getEnemiesFloatPos returns an array of floats grouped as triples float[0]
	 * represents the enemy type float[1] represents the enemy's X coordinate
	 * float[2] represents the enemy's Y coordinate
	 * 
	 * After checking both enemies exist, the difference of their X and Y
	 * coordinates are returned in the following format. float[0] enemy1.x
	 * float[1] enemy1.y float[2] enemy2.x etc.
	 * 
	 * The difference in distance approximates the velocity of the enemy. Enemy
	 * shuffling is a potential problem that can only be solved by enemy
	 * tracking which is outside of the scope of this class.
	 */
	public double[] getEnemyVelocities() {
		double[] ret = new double[6];
		ret[0] = ret[1] = ret[2] = ret[3] = ret[4] = ret[5] = 0.0;
		if ((this.prevObservation.getEnemiesFloatPos().length >= 3)
				&& (this.curObservation.getEnemiesFloatPos().length >= 3)
				&& (Math.abs(this.prevObservation.getEnemiesFloatPos()[0]) > 0.0001)
				&& (Math.abs(this.curObservation.getEnemiesFloatPos()[0]) > 0.0001)) {
			ret[0] = this.curObservation.getEnemiesFloatPos()[1]
					- this.prevObservation.getEnemiesFloatPos()[1];
			ret[1] = this.curObservation.getEnemiesFloatPos()[2]
					- this.prevObservation.getEnemiesFloatPos()[2];
		}
		if ((this.prevObservation.getEnemiesFloatPos().length >= 6)
				&& (this.curObservation.getEnemiesFloatPos().length >= 6)
				&& (Math.abs(this.prevObservation.getEnemiesFloatPos()[3]) > 0.0001)
				&& (Math.abs(this.curObservation.getEnemiesFloatPos()[3]) > 0.0001)) {
			ret[2] = this.curObservation.getEnemiesFloatPos()[4]
					- this.prevObservation.getEnemiesFloatPos()[4];
			ret[3] = this.curObservation.getEnemiesFloatPos()[5]
					- this.prevObservation.getEnemiesFloatPos()[5];
		}
		if ((this.prevObservation.getEnemiesFloatPos().length >= 9)
				&& (this.curObservation.getEnemiesFloatPos().length >= 9)
				&& (Math.abs(this.prevObservation.getEnemiesFloatPos()[6]) > 0.0001)
				&& (Math.abs(this.curObservation.getEnemiesFloatPos()[6]) > 0.0001)) {
			ret[4] = this.curObservation.getEnemiesFloatPos()[7]
					- this.prevObservation.getEnemiesFloatPos()[7];
			ret[5] = this.curObservation.getEnemiesFloatPos()[8]
					- this.prevObservation.getEnemiesFloatPos()[8];
		}

		return ret;
	}

	public double[] getCoinsPos() {
		return getClosestOfObject(2, ID_COIN); // Returns the position of the 2
												// closest coins
	}

	private double[] getClosestOfObject(int i, int type) {
		double objectPos[] = new double[i * 2];
		// Map (distance -> type), sorting by distance
		PriorityQueue<LevelObject> pq = new PriorityQueue<LevelObject>();
		byte[][] levelObs = this.curObservation.getLevelSceneObservation();
		for (int x = 0; x < 22; x++) {
			for (int y = 0; y < 22; y++) {
				if (levelObs[x][y] == type)
					pq.add(new LevelObject(x, y, levelObs[x][y]));
			}
		}
		LevelObject[] iObjects = new LevelObject[i];
		for (int j = 0; j < i; j++) {
			if (pq.size() > 0) {
				iObjects[j] = pq.remove();
			} else {
				iObjects[j] = null;
			}
		}
		for (int j = 0; j < i; j++) {
			if (iObjects[j] != null) {
				objectPos[j * 2] = iObjects[j].getxOffset();
				objectPos[j * 2 + 1] = iObjects[j].getyOffset();
			} else {
				objectPos[j * 2] = 1.0;
				objectPos[j * 2 + 1] = 1.0;
			}
		}
		return objectPos;
	}

	public void updateEnvironment(Environment observation) {
		prevObservation = curObservation;
		if (this.prevObservation instanceof NullEnvironment) {
			prevObservation = observation;
		}
		curObservation = observation;
	}

	public double[] getFeatureSet() {
		if (curObservation instanceof NullEnvironment
				|| prevObservation instanceof NullEnvironment) {
			return new double[40];
		}

		List<Double> results = new ArrayList<Double>();
		for (double d : getMarioVelocity())
			results.add(d);
		// results.add(getMarioState());
		// results.add(getClockTime());
		// results.add(getDistanceToFinish());
		for (double d : getHeights())
			results.add(d);
		// for (double d : getEnemies())
		// results.add(d);
		// for (double d : getEnemyPositions())
		// results.add(d);
		results.add(getEnemyPositions()[0]);
		// for (double d : getEnemyVelocities())
		// results.add(d);
		for (double d : getCoinsPos())
			results.add(d);
		results.add(getCanJump());
		// results.add(getUpPressed());
		results.add(getLeftPressed());
		results.add(getRightPressed());
		results.add(getDownPressed());
		results.add(getSpeedPressed());
		results.add(getJumpPressed());
		results.add(startedJump());
		results.add(canGoRight());
		results.add(holeToWithin3());
		double[] result = new double[results.size()];
		for (int i = 0; i < results.size(); i++)
			result[i] = results.get(i);
		return result;
	}

	double getCanJump() {
		return (curObservation.isMarioOnGround()) ? 1.0 : 0.0;
	}

	double canGoRight() {

//		System.out.println("1 " + getHeights()[0]);
//		System.out.println("2 " + getHeights()[1]);
//		System.out.println("3 " + getHeights()[2]);
//		System.out.println("4 " + getHeights()[3]);
//		System.out.println("5 " + getHeights()[4]);
//		System.out.println("6 " + getHeights()[5]);
//		System.out.println("7 " + getHeights()[6]);
//		System.out.println("8 " + getHeights()[7]);
//		System.out.println("9 " + getHeights()[8]);
//		System.out.println("T " + getHeights()[9]);
		return getHeights()[1] < 0.5 ? 0.0 : 1.0;
	}

	double holeToWithin3() {
		double heights[] = getHeights();
		if (heights[1] > .9)
			return 1.0;
		if (heights[2] > .9)
			return 1.0;
		if (heights[3] > .9)
			return 1.0;
		return 0.0;

	}

	// Up Pressed
	double getUpPressed() {
		return (curObservation.getKeysPressed()[Mario.KEY_UP]) ? 1.0 : 0.0;
	}

	// Left Pressed
	double getLeftPressed() {
		return (curObservation.getKeysPressed()[Mario.KEY_LEFT]) ? 1.0 : 0.0;
	}

	// Right Pressed
	double getRightPressed() {
		return (curObservation.getKeysPressed()[Mario.KEY_RIGHT]) ? 1.0 : 0.0;
	}

	// Down Pressed
	double getDownPressed() {
		return (curObservation.getKeysPressed()[Mario.KEY_DOWN]) ? 1.0 : 0.0;
	}

	// Speed/Fire Pressed
	double getSpeedPressed() {
		return (curObservation.getKeysPressed()[Mario.KEY_SPEED]) ? 1.0 : 0.0;
	}

	// Jump Pressed
	double getJumpPressed() {
		return (curObservation.getKeysPressed()[Mario.KEY_JUMP]) ? 1.0 : 0.0;
	}

	// started Jump
	double startedJump() {
		return (!prevObservation.getKeysPressed()[Mario.KEY_JUMP] && curObservation
				.getKeysPressed()[Mario.KEY_JUMP]) ? 1.0 : 0.0;
	}

}
